/*
 *  Player Java Client 3 - SimulationInterface.java
 *  Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * $Id: SimulationInterface.java 125 2011-03-24 02:24:05Z corot $
 *
 */
package javaclient3;

import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;

import javaclient3.structures.PlayerMsgHdr;
import javaclient3.structures.PlayerPose2d;
import javaclient3.structures.PlayerPose3d;
import javaclient3.structures.simulation.PlayerSimulationData;
import javaclient3.structures.simulation.PlayerSimulationPose2dReq;
import javaclient3.structures.simulation.PlayerSimulationPose3dReq;
import javaclient3.xdr.OncRpcException;
import javaclient3.xdr.XdrBufferDecodingStream;
import javaclient3.xdr.XdrBufferEncodingStream;


/**
 * Player devices may either be real hardware or virtual devices generated by
 * a simulator such as Stage or Gazebo. This interface provides direct access
 * to a simulator.
 * <br><br>
 * This interface doesn't do much yet. It is in place to later support things
 * like pausing and restarting the simulation clock, saving and loading, etc.
 * It is documented because it is used by the stg_simulation driver; required
 * by all stageclient drivers (stg_*).
 * @author Radu Bogdan Rusu
 * @version
 * <ul>
 *      <li>v3.0 - Player 3.0 supported
 * </ul>
 */
public class SimulationInterface extends PlayerDevice {

    private static final boolean isDebugging = PlayerClient.isDebugging;

    // Logging support
    private Logger logger = Logger.getLogger (SimulationInterface.class.getName ());

    private PlayerSimulationData       data;
    private boolean                    readyData = false;

    private PlayerSimulationPose2dReq  psp2dreq;
    private PlayerSimulationPose3dReq  psp3dreq;
    private boolean                    readyPsp2dreq  = false;
    private boolean                    readyPsp3dreq  = false;


    /**
     * Constructor for SimulationInterface.
     * @param pc a reference to the PlayerClient object
     */
    public SimulationInterface (PlayerClient pc) { super(pc); }

    /**
     * Read the simulation data
     */
    public synchronized void readData (PlayerMsgHdr header) {
        try {
            this.timestamp = header.getTimestamp ();

            data = new PlayerSimulationData ();
            // Buffer for reading data
            byte[] buffer = new byte[4];
            is.readFully (buffer);

            // Begin decoding the XDR buffer
            XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
            xdr.beginDecoding ();
            data.setData (xdr.xdrDecodeByte ());
            xdr.endDecoding   ();
            xdr.close ();

            readyData = true;
        } catch (IOException e) {
            throw new PlayerException
                ("[Simulation] : Error reading payload: " +
                        e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException
                ("[Simulation] : Error while XDR-decoding payload: " +
                        e.toString(), e);
        }
    }

    /**
     * Get the 2D pose of a named simulation object.
     * @return an object of type PlayerSimulationPose2dReq containing the required pose data
     */
    public PlayerSimulationPose2dReq getSimulationPose2D () { return psp2dreq; }

    /**
     * Get the 3D pose of a named simulation object.
     * @return an object of type PlayerSimulationPose3dReq containing the required pose data
     */
    public PlayerSimulationPose3dReq getSimulationPose3D () { return psp3dreq; }


    /**
     * Check if 2D pose data is available.
     * @return true if ready, false if not ready
     */
    public boolean isPose2DReady () {
        if (readyPsp2dreq) {
            readyPsp2dreq = false;
            return true;
        }
        return false;
    }

    /**
     * Check if 3D pose data is available.
     * @return true if ready, false if not ready
     */
    public boolean isPose3DReady () {
        if (readyPsp3dreq) {
            readyPsp3dreq = false;
            return true;
        }
        return false;
    }


    /**
     * Get the data.
     * @return an object of type PlayerSimulationData containing the required data
     */
    public PlayerSimulationData getData () { return this.data; }

    /**
     * Check if data is available.
     * @return true if ready, false if not ready
     */
    public boolean isDataReady () {
        if (readyData) {
            readyData = false;
            return true;
        }
        return false;
    }


    /**
     * Placeholder for command.
     * <br><br>
     * See the player_simulation_cmd structure from player.h
     * @param cmd a single byte of as-yet-unspecified command.
     */
    public void setPosition (byte cmd) {
        try {
            sendHeader (PLAYER_MSGTYPE_CMD, 0, 4);
            XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (4);
            xdr.beginEncoding (null, 0);
            xdr.xdrEncodeByte ((byte)cmd);
            xdr.endEncoding ();
            os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
            xdr.close ();
            os.flush ();
        } catch (IOException e) {
            throw new PlayerException
                ("[Simulation] : Couldn't send command: " +
                        e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException
                ("[Simulation] : Error while XDR-encoding command: " +
                        e.toString(), e);
        }
    }

    /**
     * Configuration request: set 2D pose of a named simulation object.
     *<br><br>
     * To set the pose of an object in a simulator, send a
     * PLAYER_SIMULATION_REQ_SET_POSE2D request. Response will be null.
     * <br><br>
     * See the player_simulation_pose2d_req structure from player.h
     */
    public void set2DPose (String identifier, PlayerPose2d pp) {
        String temp = identifier;
        if (identifier.length () > PLAYER_SIMULATION_IDENTIFIER_MAXLEN)
            temp = identifier.substring (0, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
        try {
            int leftOvers = 0;
            // Take care of the residual zero bytes
            if ((temp.length () % 4) != 0)
                leftOvers = 4 - (temp.length () % 4);

            int size = 8 + 24 + temp.length () + leftOvers;

            sendHeader (PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_SET_POSE2D, size);
            XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (8);
            xdr.beginEncoding (null, 0);
            xdr.xdrEncodeInt (temp.length () + 1);
            xdr.xdrEncodeInt (temp.length () + 1);
            xdr.endEncoding ();
            os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
            xdr.close ();

            os.writeBytes (temp);
            byte[] buf = new byte[leftOvers];
            os.write (buf, 0, leftOvers);

            xdr = new XdrBufferEncodingStream (24);
            xdr.beginEncoding (null, 0);
            xdr.xdrEncodeDouble (pp.getPx ());
            xdr.xdrEncodeDouble (pp.getPy ());
            xdr.xdrEncodeDouble (pp.getPa ());
            xdr.endEncoding ();
            os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
            xdr.close ();
            os.flush ();
        } catch (IOException e) {
            throw new PlayerException
                ("[Simulation] : Couldn't request " +
                        "PLAYER_SIMULATION_REQ_SET_POSE2D: " +e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException
                ("[Simulation] : Error while XDR-encoding SET_POSE2D request: "
                        + e.toString(), e);
        }
    }

    /* helper function for get2Dpose and get3Dpose bellow */
    private void getPose(String identifier, String type, int cmd, int extraLen) {
        String temp = identifier;
        if (identifier.length () > PLAYER_SIMULATION_IDENTIFIER_MAXLEN)
            temp = identifier.substring (0, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
        try {
            int leftOvers = 0;
            // Take care of the residual zero bytes
            if ((temp.length () % 4) != 0)
                leftOvers = 4 - (temp.length () % 4);

            int size = 8 + extraLen + temp.length () + leftOvers;

            sendHeader (PLAYER_MSGTYPE_REQ, cmd, size);
            XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (8);
            xdr.beginEncoding (null, 0);
            xdr.xdrEncodeInt (temp.length () + 1);    // name_count
            xdr.xdrEncodeInt (temp.length () + 1);    // array_count
            xdr.endEncoding ();
            os.write (xdr.getXdrData (), 0, xdr.getXdrLength ());
            xdr.close ();

            os.writeBytes (temp);
            byte[] buf = new byte[leftOvers];
            os.write (buf, 0, leftOvers);

            buf = new byte[extraLen];
            os.write (buf, 0, extraLen);
            os.flush ();
        } catch (IOException e) {
            throw new PlayerException
                ("[Simulation] : Couldn't request " +
                        "PLAYER_SIMULATION_REQ_GET_POSE" + type + ": " +e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException
                ("[Simulation] : Error while XDR-encoding GET_POSE" + type + " request: "
                        + e.toString(), e);
        }
    }

    /**
     * Configuration request: get 2D pose of a named simulation object.
     *<br><br>
     * To set or get the pose of an object in a simulator, use this message type. If the
     * subtype is PLAYER_SIMULATION_GET_POSE2D, the server will attempt to locate the
     * named object and reply with the same packet with (x,y,a) filled in. For all message
     * subtypes, if the named object does not exist, or some other error occurs, the request
     * should reply NACK.<br><br>
     * See the player_simulation_pose2d_req structure from player.h
     * @param identifier the identifier of the object we want to locate
     */
    public void get2DPose (String identifier) {
        getPose(identifier, "2D", PLAYER_SIMULATION_REQ_GET_POSE2D, 3*8);
    }

    /**
     * Configuration request: get 3D pose of a named simulation object.
     */
    public void get3DPose (String identifier) {
        getPose(identifier, "3D", PLAYER_SIMULATION_REQ_GET_POSE3D, 7*8);
    }


    private void handleResponse2D() throws IOException, OncRpcException
    {
        psp2dreq = new PlayerSimulationPose2dReq ();

        // Buffer for reading name_count
        byte[] buffer = new byte[8];
        is.readFully (buffer);

        // Begin decoding the XDR buffer
        XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
        xdr.beginDecoding ();
        int nameCount = xdr.xdrDecodeInt ();
        xdr.endDecoding   ();
        xdr.close ();

        buffer = new byte[nameCount];
        is.readFully (buffer, 0, nameCount);

        String name = new String (buffer).substring (0, nameCount-1);
        psp2dreq.setName (name.toCharArray());

        // Take care of the residual zero bytes
        if ((nameCount % 4) != 0)
            is.readFully (buffer, 0, 4 - (nameCount % 4));

        // Buffer for reading pose
        buffer = new byte[3*8];
        is.readFully (buffer);

        PlayerPose2d pp = new PlayerPose2d ();
        // Begin decoding the XDR buffer
        xdr = new XdrBufferDecodingStream (buffer);
        xdr.beginDecoding ();
        pp.setPx (xdr.xdrDecodeDouble ());
        pp.setPy (xdr.xdrDecodeDouble ());
        pp.setPa (xdr.xdrDecodeDouble ());
        xdr.endDecoding   ();
        xdr.close ();

        psp2dreq.setPose (pp);

        readyPsp2dreq  = true;
    }

    private void handleResponse3D() throws IOException, OncRpcException
    {
        psp3dreq = new PlayerSimulationPose3dReq ();

        // Buffer for reading name_count
        byte[] buffer = new byte[8];
        is.readFully (buffer);

        // Begin decoding the XDR buffer
        XdrBufferDecodingStream xdr = new XdrBufferDecodingStream (buffer);
        xdr.beginDecoding ();
        int nameCount = xdr.xdrDecodeInt ();
        xdr.endDecoding   ();
        xdr.close ();

        buffer = new byte[nameCount];
        is.readFully (buffer, 0, nameCount);

        String name = new String (buffer).substring (0, nameCount-1);
        psp3dreq.setName (name.toCharArray());

        // Take care of the residual zero bytes
        if ((nameCount % 4) != 0)
            is.readFully (buffer, 0, 4 - (nameCount % 4));

        // Buffer for reading pose (7 * 8 = 56)
        buffer = new byte[56];
        is.readFully (buffer, 0, 56);

        PlayerPose3d pp = new PlayerPose3d ();
        // Begin decoding the XDR buffer
        xdr = new XdrBufferDecodingStream (buffer);
        xdr.beginDecoding ();
        // The position 3D
        pp.setPx     (xdr.xdrDecodeDouble ());
        pp.setPy     (xdr.xdrDecodeDouble ());
        pp.setPz     (xdr.xdrDecodeDouble ());
        pp.setProll  (xdr.xdrDecodeDouble ());
        pp.setPpitch (xdr.xdrDecodeDouble ());
        pp.setPyaw   (xdr.xdrDecodeDouble ());
        psp3dreq.setPose (pp);
        // The simulation time
        psp3dreq.setSimtime(xdr.xdrDecodeDouble ());
        xdr.endDecoding   ();
        xdr.close ();

        readyPsp3dreq  = true;
    }

    /**
     * Handle acknowledgement response messages.
     * @param header Player header
     */
    protected void handleResponse (PlayerMsgHdr header) {
        try {
            switch (header.getSubtype ()) {
                case PLAYER_SIMULATION_REQ_GET_POSE2D:
                    handleResponse2D();
                    break;
                case PLAYER_SIMULATION_REQ_GET_POSE3D:
                    handleResponse3D();
                    break;
                case PLAYER_SIMULATION_REQ_SET_POSE2D:
                    break;
                default:
                    if (isDebugging)
                        logger.log (Level.FINEST, "[Simulation][Debug] : " +
                                "Unexpected response " + header.getSubtype () +
                                " of size = " + header.getSize ());
                    break;
            }
        } catch (IOException e) {
            throw new PlayerException
                ("[Simulation] : Error reading payload: " +
                        e.toString(), e);
        } catch (OncRpcException e) {
            throw new PlayerException
                ("[Simulation] : Error while XDR-decoding payload: " +
                        e.toString(), e);
        }
    }
}
